function [] = DrawObjectiveFunction( TF, paramname,weights,w,h,label)
% given a set of fundamental matrices, their correct intrinsic parameters
% (where we assume almost constant intrinsics) and the correlations and the
% number of parameters to draw and a type of objective function, this
% function will draw that objective function given those parameters, useful
% for assessing the usefuless of a new objective function and its effects
% on the estimation of the intrinsic parameters

% the list of the param name is as follows:
% f= only focal length
% r= only focal length
%far = focal length and aspect ratio
% foc = focal length and then an error in the optical center
% focx= focal length and the x component of the optical center
% focy = focal length and the y component of the optical center
% oc = the two optical axis components

% the type of the objective function can be found from the list in the next
% function
if(size(weights,1)<2)
    weights=ones(size(TF,2),1);
end
weights=sqrt(weights);
weights=weights*(1/sum(weights));
correctK=[ w 0 w/2 ; 0 w h/2; 0 0 1 ];

if(strcmpi(paramname,'f')==1)
    drawFocalObjective(correctK,TF,weights,label);
elseif(strcmpi(paramname,'far')==1)
    drawFocalARObjective(correctK,TF,weights,label);
elseif(strcmpi(paramname,'ff')==1)
    drawFocalfocbjective(correctK,TF,weights,label);
elseif(strcmpi(paramname,'ar')==1)
    drawARObjective(correctK,TF,weights,label);
elseif(strcmpi(paramname,'foc')==1)
    drawFocalOC(correctK,TF,weights,label);
elseif(strcmpi(paramname,'focx')==1)
    drawFocalOCX(correctK,TF,weights,label);
elseif(strcmpi(paramname,'focy')==1)
    drawFocalOCY(correctK,TF,weights,label);
elseif(strcmpi(paramname,'oc')==1)
    drawOCXY(correctK,TF,weights,label);
else
    error('the error function is invalid');
end







end


function [] =  drawFocalObjective(correctK,TF, weights,label,w,h)
numpoints=400;

fmax=2000;
fmin=250;

functionname='regularweighted';
ar=correctK(2,2)/correctK(1,1);

t=fmin:((fmax-fmin)/(numpoints-1)):fmax;
objective=zeros(numpoints,1);

bestf=0;
scoremax=10000000;

for i=1:(numpoints)
    myK= correctK;
    
    myK(1,1)=t(i);
    myK(2,2)=ar*myK(1,1);
    
    error=calcTempSVNDErr(myK,TF,weights );
    
    objective(i,1)= error;
    if(error<scoremax)
        bestf=myK(1,1);
        scoremax=error;
    end
end

plot( t,objective);
title({['lowest point f is :' num2str(bestf)],['algorithm= ' label]});
xlabel('focal length');
ylabel('objective function value');


end

function [] =  drawARObjective(correctK,TF, weights,label,w,h)
numpoints=200;

functionname='regularweighted';


armin=0.2;
armax=2.2;

functionname='regularweighted';

art=(armin):((armax-armin)/(numpoints-1)):(armax);

objective=zeros(numpoints);


bestar=0;
scoremax=10000000;


for i=1:(numpoints)
    myK= correctK;
    
    
    myK(2,2)=art(i)*myK(1,1);
    
    error=calcTempSVNDErr(myK,TF,weights );
    
    objective(i)=  error;
    if(error<scoremax)
        
        scoremax=error;
        bestar=art(i);
        
    end
    
end

%
plot(art,objective);
title({['lowest point ar is :' num2str(bestar)],['algorithm= ' label]});
xlabel('focal length');
ylabel('objective function value');
end
function [] =  drawFocalfocbjective(correctK,TF, weights,label,w,h)
numpoints=200;

fmax=2000;
fmin=340;

functionname='regularweighted';

ft1=fmin:((fmax-fmin)/(numpoints-1)):fmax;
ft2=fmin:((fmax-fmin)/(numpoints-1)):fmax;

objective=zeros(numpoints,numpoints);
X=zeros(numpoints,numpoints);
Y=zeros(numpoints,numpoints);
countr=0;
bestf1=0;
bestf2=0;
scoremax=10000000;
for j=1:(numpoints)
    for i=1:(numpoints)
        myK= correctK;
        
        myK(1,1)=ft1(j);
        myK(2,2)=ft2(i);
        
        error=calcTempSVNDErr(myK,TF,weights );
        
        
        
        objective(j,i)= error;
        X(j,i)= myK(1,1);
        Y(j,i)= myK(2,2) ;
        if(error<scoremax)
            bestf1=myK(1,1);
            bestf2=myK(2,2);
            scoremax=error;
            
        end
        
    end
end
%
[bestf1  bestf2]
mesh(X,Y,objective);
%display(['best score is ' num2str(scoremax)]);
title({['lowest point f1 is :' num2str(bestf1) ' and best f2 is :' num2str(bestf2)],['algorithm= ' label]});
xlabel('focal length');
ylabel('aspect ratio length');
zlabel('objective function value');

end
function [] =  drawFocalARObjective(correctK,TF, weights,label,w,h)
numpoints=200;
armin=0.8;
armax=1.2;
fmax=2000;
fmin=340;

functionname='regularweighted';

ft=fmin:((fmax-fmin)/(numpoints-1)):fmax;
art=(armin):((armax-armin)/(numpoints-1)):(armax);

objective=zeros(numpoints,numpoints);
X=zeros(numpoints,numpoints);
Y=zeros(numpoints,numpoints);
countr=0;
bestf=0;
bestar=0;
scoremax=10000000;
for j=1:(numpoints)
    for i=1:(numpoints)
        myK= correctK;
        
        myK(1,1)=ft(j);
        myK(2,2)=art(i)*myK(1,1);
        
        error=calcTempSVNDErr(myK,TF,weights );
        
        
        
        objective(j,i)= error;
        X(j,i)= myK(1,1);
        Y(j,i)= myK(2,2)/myK(1,1) ;
        if(error<scoremax)
            bestf=myK(1,1);
            scoremax=error;
            bestar=myK(2,2)/myK(1,1);
            
        end
        
    end
end
%
mesh(X,Y,objective);
%display(['best score is ' num2str(scoremax)]);
title({['lowest point f is :' num2str(bestf) ' and best AR is : ' num2str(bestar)],['algorithm= ' label]});
xlabel('focal length');
ylabel('aspect ratio length');
zlabel('objective function value');

end

function [] = drawFocalOC(correctK,TF, weights,label,w,h)
numpoints=400;
focalside=550;
functionname='regularweighted';
ocside=200;

truear=correctK(2,2)/correctK(1,1);
truefocal=correctK(1,1);


ft=(truefocal-focalside):((focalside*2)/(numpoints-1)):(truefocal+focalside);
ocdev=(-ocside):((ocside*2)/(numpoints-1)):(ocside);

objective=zeros(numpoints,numpoints);

bestf=0;
besocdev=0;
scoremax=10000000;

for j=1:(numpoints)
    for i=1:(numpoints)
        myK= correctK;
        
        myK(1,1)=ft(j);
        myK(2,2)=truear*myK(1,1);
        
        myK(1,3)=myK(1,3)+ocdev(i);
        myK(2,3)=myK(2,3)+ocdev(i);
        
        error=calcTempSVNDErr(myK,TF,weights );
        
        
        objective(j,i)= error;
        if(error<scoremax)
            scoremax=error;
            bestf=myK(1,1);
            besocdev=ocdev(i);
        end
        
    end
end

mesh(ft,ocdev,objective);

title({['lowest point f is :' num2str(bestf) ' and best OC deviation is : ' num2str(besocdev)],['algorithm= ' label]});
xlabel('focal length');
ylabel('optical center dev');
zlabel('objective function value');

end

function [] = drawFocalOCX(correctK,TF, weights,label,w,h)

numpoints=400;

xmin=0;
xmax=correctK(1,3)*2;
fmax=2000;
fmin=140;

truear=correctK(2,2)/correctK(1,1);

functionname='regularweighted';

ft=fmin:((fmax-fmin)/(numpoints-1)):fmax;
ocxdev=(xmin):((xmax-xmin)/(numpoints-1)):(xmax);

objective=zeros(numpoints,numpoints);
X=zeros(numpoints,numpoints);
Y=zeros(numpoints,numpoints);

bestf=0;
besocx=0;
scoremax=10000000;

for j=1:(numpoints)
    for i=1:(numpoints)
        myK= correctK;
        
        myK(1,1)=ft(j);
        myK(2,2)=truear*myK(1,1);
        
        myK(1,3)=ocxdev(i);
        
        error=calcTempSVNDErr(myK,TF,weights );

        
        objective(j,i)= error;
        X(j,i)= ft(j);
        Y(j,i)= ocxdev(i);
        if(error<scoremax)
            bestf=myK(1,1);
            scoremax=error;
            besocx=myK(1,3);
        end
        
    end
end

mesh(X,Y,objective);
title({['lowest point f is :' num2str(bestf) ' and best OCx is : ' num2str(besocx)],['algorithm= ' label]});
xlabel('focal length');
ylabel('truex');
zlabel('objective function value');
end


function [] = drawFocalOCY(correctK,TF, weights,label,w,h)
numpoints=400;

ymin=0;
ymax=TF(2,3)*2;
fmax=2000;
fmin=140;

functionname='regularweighted';

ft=fmin:((fmax-fmin)/(numpoints-1)):fmax;
ocydev=(ymin):((ymax-ymin)/(numpoints-1)):(ymax);


objective=zeros(numpoints,numpoints);
X=zeros(numpoints,numpoints);
Y=zeros(numpoints,numpoints);
bestf=0;
besocy=0;
scoremax=10000000;


for j=1:(numpoints)
    for i=1:(numpoints)
        myK= correctK;
        
        myK(1,1)=ft(j);
        myK(2,2)=truear*myK(1,1);
        
        myK(2,3)=ocydev(i);
        
        error=calcTempSVNDErr(myK,TF,weights );
        
        objective(j,i)= error;
        X(j,i)= ft(j);
        Y(j,i)= ocydev(i);
        if(error<scoremax)
            scoremax=error;
            besocy=myK(2,3);
            bestf=myK(1,1);
        end
        
    end
end

mesh(X,Y,objective);
title({['lowest point f is :' num2str(bestf) ' and best OCx is : ' num2str(besocy)],['algorithm= ' label]});
xlabel('focal length');
ylabel('truex');
zlabel('objective function value');
end


function [] = drawOCXY(correctK,TF, weights,label,w,h)
numpoints=400;

ymin=0;
ymax=TF(2,3)*2;
xmin=0;
xmax=TF(1,3)*2;

functionname='regularweighted';

ocxdev=(xmin):((xmax-xmin)/(numpoints-1)):(xmax);
ocydev=(ymin):((ymax-ymin)/(numpoints-1)):(ymax);

objective=zeros(numpoints,numpoints);

besocx=0;
besocy=0;
scoremax=10000000;
X=zeros(numpoints,numpoints);
Y=zeros(numpoints,numpoints);

for j=1:(numpoints)
    for i=1:(numpoints)
        myK= correctK;
        
        myK(1,3)=ocxdev(j);
        myK(2,3)=ocydev(i);
        
        error=calcTempSVNDErr(myK,TF,weights );
        
        
        X(j,i)= ocxdev(j);
        Y(j,i)= ocydev(i);
        
        objective(j,i)=  error;
        if(error<scoremax)
            scoremax=error;
            besocy=myK(2,3);
            besocx=myK(1,3);
        end
    end
end

mesh(ocxdev,ocydev,objective);
title({[' best OCx is : ' num2str(besocx) ' best OCy is : ' num2str(besocy)],['algorithm= ' label]});
xlabel('x axis');
ylabel('y axis');
zlabel('objective function value');
end

function [] = drawFocalAROCFOC(correctK,TF, weights,label,w,h)

end
function [error]=calcTempSVNDErr(K,F,weights )

n=size(F,2);
ERRZ=zeros(n,1);
KT=K';

for i=1:n
    ERRZ(i,1) =weights(i,1)* computerEssentialErrorSVD(F{1,i},K,KT);
end

ERRZ=ERRZ.^2;
D=sum(abs(ERRZ));
error=D;


end

